function distortedPoints = distortPoints(points, intrinsicMatrix, ...
    radialDistortion, tangentialDistortion)
%

%   Copyright 2014-2020 The MathWorks, Inc.

%#codegen

% unpack the intrinisc matrix
cx = intrinsicMatrix(1, 3);
cy = intrinsicMatrix(2, 3);
fx = intrinsicMatrix(1, 1);
fy = intrinsicMatrix(2, 2);
skew = intrinsicMatrix(1, 2);

% center the points
center = [cx, cy];
centeredPoints = bsxfun(@minus, points, center);

% normalize the points
yNorm = centeredPoints(:, 2, :) ./ fy;
xNorm = centeredPoints(:, 1, :) ./ fx;

% compute radial distortion
r2 = xNorm .^ 2 + yNorm .^ 2;
r4 = r2 .* r2;
% r6 = r2 .* r4;

k = zeros(1, 2, 'like', radialDistortion);
k(1:2) = radialDistortion(1:2);

alpha = k(1) * r2 + k(2) * r4 ;

% compute tangential distortion
p = tangentialDistortion;
xyProduct = xNorm .* yNorm;
dxTangential = 2 * p(1) * xyProduct + p(2) * (r2 + 2 * xNorm .^ 2);
dyTangential = p(1) * (r2 + 2 * yNorm .^ 2) + 2 * p(2) * xyProduct;

% apply the distortion to the points
normalizedPoints = [xNorm, yNorm];
distortedNormalizedPoints = normalizedPoints + normalizedPoints .* [alpha, alpha] + ...
    [dxTangential, dyTangential];

% convert back to pixels
distortedPointsX = distortedNormalizedPoints(:, 1, :) * fx + cx +skew * distortedNormalizedPoints(:, 2, :);
distortedPointsY = distortedNormalizedPoints(:, 2, :) * fy + cy;


distortedPoints = [distortedPointsX, distortedPointsY];


end